// ==============================================
// =      U.A.V.P Brushless UFO Controller      =
// =           Professional Version             =
// = Copyright (c) 2007 Ing. Wolfgang Mahringer =
// ==============================================
//
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 2 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License along
//  with this program; if not, write to the Free Software Foundation, Inc.,
//  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//
// ==============================================
// =  please visit http://www.uavp.org          =
// =               http://www.mahringer.co.at   =
// ==============================================
//http://www.patentstorm.us/patents/7430460/claims.html   
//ROT=K*[Ay*COS(Bank)+Az*SIN(Bank)]/V
// Accelerator sensor routine

#pragma codepage=2
#include "c-ufo.h"
#include "bits.h"

// Math Library
#include "mymath16.h"

// read all acceleration values from LISL sensor
// and compute correction adders (Rp, Np, Vud)
void CheckLISL(void)
{
	long nila1@nilarg1;
	
	if( !_UseLISL )
	{	
		Rp = Np = 0;
		return;	
	}
		// GS: what's reading status good for ??	
	ReadLISL(LISL_STATUS + LISL_READ);
	// the LISL registers are in order here!!
	Rp.low8  = (int)ReadLISL(LISL_OUTX_L + LISL_INCR_ADDR + LISL_READ);  // left right 
	Rp.high8 = (int)ReadLISLNext();
	Tp.low8  = (int)ReadLISLNext(); // up/down 
	Tp.high8 = (int)ReadLISLNext();
	Np.low8  = (int)ReadLISLNext();	// front/back
	Np.high8 = (int)ReadLISLNext();
	
	// These two lines reset the FreeFall interupt again -- for testing purposes
	// ReadLISL(LISL_STATUS + LISL_READ);
	// ReadLISL(LISL_FF_ACK + LISL_READ); // read ff ack

	LISL_CS = 1;	// end transmission
// 1 unit is 1/4096 of 2g = 1/2048g

	// The MiddleYX values are from the UAVP Setup -- stored in the config EEPROM section 
	Rp -= MiddleLR;
	Tp -= MiddleUD;
	Np -= MiddleFB;
	

	Tp -= 1024;	// subtract 1g

// UpDownVelocity integrates  up down acceleration  == up down velocity 
// The Up down stuf doesn't work  See comments in MixANdLimit

	UpDown_Velocity += Tp;

	Tp = UpDown_Velocity;
//	Tp += 64;
//	Tp >>= 7;  // div by 64 
	//Tp *= LinUDIntFactor;
	
	// Vud = UpDown_Velocity/64;	
	Vud = Tp.high8;

/*
	if( (BlinkCount & 0x03) == 0 )	
	{
		if( (int)Tp.high8 > Vud )
			Vud++;
			
		if( (int)Tp.high8 < Vud )
			Vud--;
		
		if( Vud >  20 ) Vud =  20;
		if( Vud < -20 ) Vud = -20;
	}
	
	if( UpDownVelocity >  10 ) UpDownVelocity -= 10;
	if( UpDownVelocity < -10 ) UpDownVelocity += 10;
	
*/	

// =====================================
// Roll-Achse
// =====================================
	Tl = Roll_Angle * 11;	// sacle to accel 
	Tl += 64;
	Tl >>= 7;
	Rp -= Tl;

	LRIntKorr = 0;
	if( Rp > 10 ) LRIntKorr = 4;                           
	if( Rp < -10 ) LRIntKorr = -4;
	 
	// this is driving the angle to 0  when not inhibited by values from above 
	if( Roll_Angle > 0 ) LRIntKorr -=4;
	if( Roll_Angle < 0 ) LRIntKorr +=4;

// =====================================
// Pitch-Achse
// =====================================
	Tl = Pitch_Angle * 11;	//Scale Angle to match Accel representation   11/128ths
	Tl += 64;
	Tl >>= 7;
	Np -= Tl;

	FBIntKorr = 0;
	if( Np > 10 ) FBIntKorr =  4;		// these values have to match the values for the decay in CalcGyroValues()
	if( Np < -10 ) FBIntKorr = -4;	
	
/*if (FBIntKorr  == 0 )	 
	LedYellow_ON;  
LedGreen_OFF;
if (Pitch_Angle == 0 )
	LedGreen_ON;
	
if (FBIntKorr  == 0 )	 
	LedYellow_ON;                       // there will be correction by by the decay values
	
if (FBIntKorr < 0  && Pitch_Angle < 0 ) // no net correction  the two values cancel each other.
	LedRed_ON;

if (FBIntKorr > 0  && Pitch_Angle > 0 )
	LedRed_ON;	

if (FBIntKorr < 0  && Pitch_Angle > 0 ) // correction by  decay + FBIntKorr  
	LedBlue_ON;

if (FBIntKorr > 0  && Pitch_Angle < 0 ) 
	LedBlue_ON;
*/	
	if( Pitch_Angle > 0 ) FBIntKorr  -= 4;		// these decay values have to match the values in  FBIntKorr 
	if( Pitch_Angle < 0 ) FBIntKorr  += 4;

}
